#include "pwm_board.hpp"
#include <glog/logging.h>

pwm_board::pwm_board(uint8_t id, rs485_v3 *rs485, agile_modbus_rtu_master_simple *modbus) : _id(id),
                                                                                            _rs485(rs485),
                                                                                            _modbus(modbus),
                                                                                            _led_brightness(100),
                                                                                            _led_color(led_color_white),
                                                                                            _led_mode(led_mode_blink_slow)
{
    if (_rs485->rs485_get_flag() != YJ_OK)
    {
        _rs485->rs485_open();
    }

    if (_rs485->rs485_get_flag() != YJ_OK)
    {
        _rs485_flag = YJ_ERROR;
        return;
    }

    if (_modbus->init() != 0)
    {
        _rs485_flag = YJ_ERROR;
        return;
    }
    _rs485_flag = YJ_OK;
}

yj_return pwm_board::read_channel(uint8_t channel, uint8_t &value) // value 单位 0-100 对应舵机 0-100%行程
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "write_channel: pwm_board init error";
        return YJ_ERROR;
    }

    if (channel > 7)
    {
        LOG(ERROR) << "write_channel: channel error";
        return YJ_ERROR;
    }

    std::vector<uint16_t> reg_data(1);

    if (_modbus->read_holding_register_multiple(_id, (0x10 + channel - 1), reg_data) != 0)
    {
        return YJ_ERROR;
    }

    value = reg_data[0];

    return YJ_OK;
}
yj_return pwm_board::write_channel(uint8_t channel, uint8_t value) // value 单位 0-100 对应舵机 0-100%行程
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "write_channel: pwm_board init error";
        return YJ_ERROR;
    }

    if (channel > 7)
    {
        LOG(ERROR) << "write_channel: channel error";
        return YJ_ERROR;
    }

    if (value > 100)
    {
        LOG(ERROR) << "write_channel: value error";
        return YJ_ERROR;
    }

    std::vector<uint16_t> reg_data(1);
    reg_data[0] = value;

    if (_modbus->write_holding_register_multiple(_id, (0x10 + channel - 1), reg_data) != 0)
    {
        return YJ_ERROR;
    }

    return YJ_OK;
}

yj_return pwm_board::set_led(led_color_en color, uint8_t brightness, led_mode_en mode)
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "set_led: pwm_board init error";
        return YJ_ERROR;
    }

    if (brightness > 255)
    {
        LOG(ERROR) << "set_led: brightness error";
        return YJ_ERROR;
    }

    std::vector<uint16_t> reg_data(5);
    reg_data[0] = ((uint32_t)color & 0xff0000) >> 16;
    reg_data[1] = ((uint32_t)color & 0x00ff00) >> 8;
    reg_data[2] = (uint32_t)color & 0x0000ff;
    reg_data[3] = brightness;
    reg_data[4] = mode;

    if (_modbus->write_holding_register_multiple(_id, 0x20, reg_data) != 0)
    {
        return YJ_ERROR;
    }

    _led_color = color;
    _led_brightness = brightness;
    _led_mode = mode;

    return YJ_OK;
}

yj_return pwm_board::set_led_brightness(uint8_t brightness) // 设置LED亮度 0-255
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "set_led_brightness: pwm_board init error";
        return YJ_ERROR;
    }

    if (brightness > 255)
    {
        LOG(ERROR) << "set_led_brightness: brightness error";
        return YJ_ERROR;
    }

    std::vector<uint16_t> reg_data(1);
    reg_data[0] = brightness;

    if (_modbus->write_holding_register_multiple(_id, 0x23, reg_data) != 0)
    {
        return YJ_ERROR;
    }

    _led_brightness = brightness;

    return YJ_OK;
}
yj_return pwm_board::set_led_mode(led_mode_en mode) // 设置LED模式
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "set_led_mode: pwm_board init error";
        return YJ_ERROR;
    }

    std::vector<uint16_t> reg_data(1);
    reg_data[0] = mode;

    if (_modbus->write_holding_register_multiple(_id, 0x24, reg_data) != 0)
    {
        return YJ_ERROR;
    }

    _led_mode = mode;

    return YJ_OK;
}
yj_return pwm_board::set_led_color(led_color_en color) // 设置LED颜色
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "set_led_color: pwm_board init error";
        return YJ_ERROR;
    }

    std::vector<uint16_t> reg_data(3);
    reg_data[0] = ((uint32_t)color & 0xff0000) >> 16;
    reg_data[1] = ((uint32_t)color & 0x00ff00) >> 8;
    reg_data[2] = (uint32_t)color & 0x0000ff;

    if (_modbus->write_holding_register_multiple(_id, 0x20, reg_data) != 0)
    {
        return YJ_ERROR;
    }

    _led_color = color;

    return YJ_OK;
}